#include "stm32f10x.h"                  // Device header
#include "SOFTIIC.h"
#include "MPU6050_Reg.h"

#define mpu_address 0xd0 

void mpu6050_w_reg(uint8_t regaddress,uint8_t data)
{
	iic_start();
	iic_sendbyte(mpu_address);
	iic_receiveack();
	iic_sendbyte(regaddress);
	iic_receiveack();
	iic_sendbyte(data);
	iic_receiveack();
	iic_stop();
}

uint8_t mpu6050_r_reg(uint8_t regaddress)
{
	uint8_t byte;
	iic_start();
	iic_sendbyte(mpu_address);
	iic_receiveack();
	iic_sendbyte(regaddress);
	iic_receiveack();
	
	iic_start();
	iic_sendbyte(mpu_address|0x01);
	iic_receiveack();
	byte=iic_receivebyte();
	iic_sendack(1);
	iic_stop();
	
	return byte;
}

void mpu6050_init(void)
{
	iic_init();
	mpu6050_w_reg(MPU6050_PWR_MGMT_1, 0x01);
	mpu6050_w_reg(MPU6050_PWR_MGMT_2, 0x00);
	mpu6050_w_reg(MPU6050_SMPLRT_DIV, 0x09);
	mpu6050_w_reg(MPU6050_CONFIG, 0x06);
	mpu6050_w_reg(MPU6050_GYRO_CONFIG, 0x18);
	mpu6050_w_reg(MPU6050_ACCEL_CONFIG, 0x18);
}

uint8_t mpu6050_getid(void)
{
	return mpu6050_r_reg(MPU6050_WHO_AM_I);
}

void mpu6050_getdata(int16_t *AccX, int16_t *AccY, int16_t *AccZ,int16_t *GyroX, int16_t *GyroY, int16_t *GyroZ)
{
	uint8_t DataH, DataL;
	
	DataH = mpu6050_r_reg(MPU6050_ACCEL_XOUT_H);
	DataL = mpu6050_r_reg(MPU6050_ACCEL_XOUT_L);
	*AccX = (DataH << 8) | DataL;
	
	DataH = mpu6050_r_reg(MPU6050_ACCEL_YOUT_H);
	DataL = mpu6050_r_reg(MPU6050_ACCEL_YOUT_L);
	*AccY = (DataH << 8) | DataL;
	
	DataH = mpu6050_r_reg(MPU6050_ACCEL_ZOUT_H);
	DataL = mpu6050_r_reg(MPU6050_ACCEL_ZOUT_L);
	*AccZ = (DataH << 8) | DataL;
	
	DataH = mpu6050_r_reg(MPU6050_GYRO_XOUT_H);
	DataL = mpu6050_r_reg(MPU6050_GYRO_XOUT_L);
	*GyroX = (DataH << 8) | DataL;
	
	DataH = mpu6050_r_reg(MPU6050_GYRO_YOUT_H);
	DataL = mpu6050_r_reg(MPU6050_GYRO_YOUT_L);
	*GyroY = (DataH << 8) | DataL;
	
	DataH = mpu6050_r_reg(MPU6050_GYRO_ZOUT_H);
	DataL = mpu6050_r_reg(MPU6050_GYRO_ZOUT_L);
	*GyroZ = (DataH << 8) | DataL;
}

